# coding=utf-8
# 2019/12/12: 首个记录
# 2020/4/1: 导入bi_common
# 2020/4/8: 支持V6；增加GnssImuSample.set_gnss_time
# 2020/4/24: 增加arrival_time（从v6开始Offset可根据Session的UTC开始时间进行同步）
# 2020/7/28: 完善枚举类型定义
# 2020/8/3: 增加枚举类型==/!=重载
# 2020/8/5: 枚举值类改为继承EnumValue
# 2020/11/6: 增加卫星时间类型，新增gnss_time_utc方法。增加V4协议数据兼容
# 2021/12/2: 支持v7
# 2021/12/16: 增加水平误差和垂直误差
# 2022/3/16: 增加行驶曲率

import bi_common as bi
from math import pi, sin, cos, atan2
from datetime import datetime, timedelta


# 位置模式
class GnssImuLocationModeValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'NORMAL'
        elif self.val == 2:
            return 'RTK_FIXED'
        elif self.val == 3:
            return 'RTK_FLOAT'
        elif self.val == 4:
            return 'RTD'
        elif self.val == 5:
            return 'IMU_ONLY'
        elif self.val == 6:
            return 'MODIFIED'
        else:
            return 'NO_LOCATION'


class GnssImuLocationMode:
    NO_LOCATION = GnssImuLocationModeValue(0)  # 无位置信息
    NORMAL = GnssImuLocationModeValue(1)  # 默认模式
    RTK_FIXED = GnssImuLocationModeValue(2)  # RTK固定解
    RTK_FLOAT = GnssImuLocationModeValue(3)  # RTK浮动解
    RTD = GnssImuLocationModeValue(4)  # RTD
    IMU_ONLY = GnssImuLocationModeValue(5)  # 仅惯导
    MODIFIED = GnssImuLocationModeValue(6)  # 后期修正

    def __init__(self):
        pass


# 卫星时间类型
class GnssTimeTypeValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'GPS'
        else:
            return 'UTC'


class GnssTimeType:
    UTC = GnssTimeTypeValue(0)  # UTC时间
    GPS = GnssTimeTypeValue(1)  # GPS时间

    def __init__(self):
        pass


# 位置与惯导样本
class GnssImuSample:
    def __init__(self):
        self.time = 0.0  # 时间戳 s
        self.location_mode = GnssImuLocationMode.NO_LOCATION  # 位置模式
        self.satellite_count_valid = False
        self.satellite_count = 0  # 卫星数量
        self.longitude_valid = False
        self.longitude = 0.0  # 车前保中心地面经度 deg 东经为正
        self.latitude_valid = False
        self.latitude = 0.0  # 车前保中心地面纬度 deg 北纬为正
        self.altitude_valid = False
        self.altitude = 0.0  # 车前保中心地面海拔 m
        self.antenna_longitude_valid = False
        self.antenna_longitude = 0.0  # 天线位置经度 deg 东经为正
        self.antenna_latitude_valid = False
        self.antenna_latitude = 0.0  # 天线位置纬度 deg 北纬为正
        self.antenna_altitude_valid = False
        self.antenna_altitude = 0.0  # 天线位置海拔 m
        self.horizontal_error_valid = False
        self.horizontal_error = 0.0  # 水平位置误差 m
        self.vertical_error_valid = False
        self.vertical_error = 0.0  # 垂直位置误差 m
        self.speed_valid = False
        self.speed = 0.0  # 车速 kph
        self.orientation_valid = False
        self.orientation = 0.0  # 朝向 deg CCW为正, 北为0, -180~180
        self.pitch_valid = False
        self.pitch = 0.0  # 俯仰角 deg 车头朝下为正
        self.roll_valid = False
        self.roll = 0.0  # 横滚角 deg 右侧朝下为正
        self.yaw_rate_valid = False
        self.yaw_rate = 0.0  # 横摆角速度 deg/s CCW为正
        self.pitch_rate_valid = False
        self.pitch_rate = 0.0  # 俯仰角速度 deg/s 车头朝下为正
        self.roll_rate_valid = False
        self.roll_rate = 0.0  # 横滚角速度 deg/s 右侧朝下为正
        self.ax_valid = False
        self.ax = 0.0  # 纵向加速度 m/s2 朝前为正
        self.ay_valid = False
        self.ay = 0.0  # 横向加速度 m/s2 朝左为正
        self.az_valid = False
        self.az = 0.0  # 天向加速度 m/s2 朝上为正
        self.curvature_valid = False
        self.curvature = 0.0  # 行驶曲率 1/m 左转为正
        self.slip_angle_valid = False
        self.slip_angle = 0.0  # 侧偏角 deg 左转时为正
        self.jerk_x_valid = False
        self.jerk_x = 0.0  # 纵向急动度 m/s3 朝前为正
        self.jerk_y_valid = False
        self.jerk_y = 0.0  # 横向急动度 m/s3 朝左为正
        self.arrival_time_valid = False
        self.arrival_time = 0.0  # 数据到达时间戳（相对Session开始时间）
        self.gnss_time_type = GnssTimeType.UTC  # 卫星时间类型
        self.gnss_time_valid = False
        self.gnss_time = datetime(1970, 1, 1, 0, 0, 0, 0)  # 卫星时间

    # 获取UTC卫星时间
    def gnss_time_utc(self):
        if self.gnss_time_valid:
            if self.gnss_time_type == GnssTimeType.UTC:
                return self.gnss_time
            elif self.gnss_time_type == GnssTimeType.GPS:
                return self.gnss_time + timedelta(seconds=-18)
        return None

    # 转通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = 'gnssimu-sample-v7@' + str(channel)
        output.time = self.time
        output.significant = 40
        output.values = []
        output.values.append(self.location_mode.val)
        output.values.append(self.longitude if self.longitude_valid else None)
        output.values.append(self.latitude if self.latitude_valid else None)
        output.values.append(self.altitude if self.altitude_valid else None)
        output.values.append(self.speed if self.speed_valid else None)
        output.values.append(self.orientation if self.orientation_valid else None)
        output.values.append(self.pitch if self.pitch_valid else None)
        output.values.append(self.roll if self.roll_valid else None)
        output.values.append(self.yaw_rate if self.yaw_rate_valid else None)
        output.values.append(self.ax if self.ax_valid else None)
        output.values.append(self.ay if self.ay_valid else None)
        output.values.append(self.az if self.az_valid else None)
        output.values.append(self.slip_angle if self.slip_angle_valid else None)
        output.values.append(self.jerk_x if self.jerk_x_valid else None)
        output.values.append(self.jerk_y if self.jerk_y_valid else None)
        output.values.append(self.satellite_count if self.satellite_count_valid else None)
        output.values.append(self.antenna_longitude if self.antenna_longitude_valid else None)
        output.values.append(self.antenna_latitude if self.antenna_latitude_valid else None)
        output.values.append(self.antenna_altitude if self.antenna_altitude_valid else None)
        output.values.append(self.gnss_time_type.val)
        output.values.append(self.arrival_time if self.arrival_time_valid else None)
        output.values.append(self.gnss_time.year if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.month if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.day if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.hour if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.minute if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.second if self.gnss_time_valid else None)
        output.values.append((self.gnss_time.microsecond * 0.001) if self.gnss_time_valid else None)
        output.values.append(self.pitch_rate if self.pitch_rate_valid else None)
        output.values.append(self.roll_rate if self.roll_rate_valid else None)
        output.values.append(self.horizontal_error if self.horizontal_error_valid else None)
        output.values.append(self.vertical_error if self.vertical_error_valid else None)
        output.values.append(self.curvature if self.curvature_valid else None)
        output.values.append(None) # Reserved 34
        output.values.append(None) # Reserved 35
        output.values.append(None) # Reserved 36
        output.values.append(None) # Reserved 37
        output.values.append(None) # Reserved 38
        output.values.append(None) # Reserved 39
        output.values.append(None) # Reserved 40
        return output

    # 设置GNSS时间，带验证
    def set_gnss_time(self, year, month, day, hour, minute, second, micro):
        self.gnss_time_valid = False
        if year < 2000 or year > 3000 or month < 1 or month > 12 or day < 1 or day > 31 or hour < 0 or hour > 23 or minute < 0 or minute > 59 or second < 0 or second > 59 or micro < 0 or micro > 999999:
            return
        self.gnss_time = datetime(year, month, day, hour, minute, second, micro)
        self.gnss_time_valid = True


def _conv_gnssimu_sample_v4(gs):
    values_count = len(gs.values)
    if values_count != 18:
        return None
    output = GnssImuSample()
    output.time = gs.time

    if gs.values[0] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[0])
    if gs.values[1] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[1])
    output.location_mode = GnssImuLocationMode.NORMAL if output.longitude_valid and output.latitude_valid else GnssImuLocationMode.NO_LOCATION
    if gs.values[2] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[3])
    if gs.values[4] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[4])
    if gs.values[5] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[5])
    if gs.values[6] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[6])
    if gs.values[7] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[7])
    if gs.values[8] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[8])
    if gs.values[9] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[9])
    if gs.values[10] is not None:
        output.az_valid = True
        output.az = float(gs.values[10])
    output.arrival_time_valid = True
    output.arrival_time = gs.time
    if gs.values[11] is not None and gs.values[12] is not None and gs.values[13] is not None and gs.values[14] is not None and gs.values[15] is not None and gs.values[16] is not None and gs.values[17] is not None:
        output.set_gnss_time(int(gs.values[11]), int(gs.values[12]), int(gs.values[13]), int(gs.values[14]), int(gs.values[15]), int(gs.values[16]), int(gs.values[17]) * 1000)
    return output


def _conv_gnssimu_sample_v5(gs):
    values_count = len(gs.values)
    if values_count != 19:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationModeValue(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    output.arrival_time_valid = True
    output.arrival_time = gs.time
    if gs.values[12] is not None and gs.values[13] is not None and gs.values[14] is not None and gs.values[15] is not None and gs.values[16] is not None and gs.values[17] is not None and gs.values[18] is not None:
        output.set_gnss_time(int(gs.values[12]), int(gs.values[13]), int(gs.values[14]), int(gs.values[15]), int(gs.values[16]), int(gs.values[17]), int(gs.values[18]) * 1000)
    return output


def _conv_gnssimu_sample_v6(gs):
    values_count = len(gs.values)
    if values_count != 28:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationModeValue(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.altitude_valid = True
        output.altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    if gs.values[12] is not None:
        output.slip_angle_valid = True
        output.slip_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[13])
    if gs.values[14] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[14])
    if gs.values[15] is not None:
        output.satellite_count_valid = True
        output.satellite_count = int(gs.values[15])
    if gs.values[16] is not None:
        output.antenna_longitude_valid = True
        output.antenna_longitude = float(gs.values[16])
    if gs.values[17] is not None:
        output.antenna_latitude_valid = True
        output.antenna_latitude = float(gs.values[17])
    if gs.values[18] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[18])
    if gs.values[19] is not None:
        output.gnss_time_type = GnssTimeTypeValue(int(gs.values[19]))
    if gs.values[20] is not None:
        output.arrival_time_valid = True
        output.arrival_time = float(gs.values[20])
    if gs.values[21] is not None and gs.values[22] is not None and gs.values[23] is not None and gs.values[24] is not None and gs.values[25] is not None and gs.values[26] is not None and gs.values[27] is not None:
        output.set_gnss_time(int(gs.values[21]), int(gs.values[22]), int(gs.values[23]), int(gs.values[24]), int(gs.values[25]), int(gs.values[26]), int(gs.values[27]) * 1000)
    return output


def _conv_gnssimu_sample_v7(gs):
    values_count = len(gs.values)
    if values_count != 40:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationModeValue(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.altitude_valid = True
        output.altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    if gs.values[12] is not None:
        output.slip_angle_valid = True
        output.slip_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[13])
    if gs.values[14] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[14])
    if gs.values[15] is not None:
        output.satellite_count_valid = True
        output.satellite_count = int(gs.values[15])
    if gs.values[16] is not None:
        output.antenna_longitude_valid = True
        output.antenna_longitude = float(gs.values[16])
    if gs.values[17] is not None:
        output.antenna_latitude_valid = True
        output.antenna_latitude = float(gs.values[17])
    if gs.values[18] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[18])
    if gs.values[19] is not None:
        output.gnss_time_type = GnssTimeTypeValue(int(gs.values[19]))
    if gs.values[20] is not None:
        output.arrival_time_valid = True
        output.arrival_time = float(gs.values[20])
    if gs.values[21] is not None and gs.values[22] is not None and gs.values[23] is not None and gs.values[24] is not None and gs.values[25] is not None and gs.values[26] is not None and gs.values[27] is not None:
        output.set_gnss_time(int(gs.values[21]), int(gs.values[22]), int(gs.values[23]), int(gs.values[24]), int(gs.values[25]), int(gs.values[26]), int(gs.values[27]) * 1000)
    if gs.values[28] is not None:
        output.pitch_rate_valid = True
        output.pitch_rate = float(gs.values[28])
    if gs.values[29] is not None:
        output.roll_rate_valid = True
        output.roll_rate = float(gs.values[29])
    if gs.values[30] is not None:
        output.horizontal_error_valid = True
        output.horizontal_error = float(gs.values[30])
    if gs.values[31] is not None:
        output.vertical_error_valid = True
        output.vertical_error = float(gs.values[31])
    if gs.values[32] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[32])
    return output


def _interpolate_angle(a1, w1, a2, w2):
    deg2rad = pi / 180
    x1 = cos(a1 * deg2rad)
    y1 = sin(a1 * deg2rad)
    x2 = cos(a2 * deg2rad)
    y2 = sin(a2 * deg2rad)
    xo = x1 * w1 + x2 * w2
    yo = y1 * w1 + y2 * w2
    if xo == 0 and yo == 0:
        return None
    return atan2(yo, xo) / deg2rad


def _interpolate_gnssimu_sample(vs1, w1, vs2, w2):
    output = GnssImuSample()
    output.time = bi.time
    output.location_mode = vs1.location_mode if w1 > w2 else vs2.location_mode
    if vs1.location_mode.val == vs2.location_mode.val:
        if vs1.longitude_valid and vs2.longitude_valid:
            output.longitude_valid = True
            output.longitude = vs1.longitude * w1 + vs2.longitude * w2
        if vs1.latitude_valid and vs2.latitude_valid:
            output.latitude_valid = True
            output.latitude = vs1.latitude * w1 + vs2.latitude * w2
        if vs1.altitude_valid and vs2.altitude_valid:
            output.altitude_valid = True
            output.altitude = vs1.altitude * w1 + vs2.altitude * w2
        if vs1.antenna_longitude_valid and vs2.antenna_longitude_valid:
            output.antenna_longitude_valid = True
            output.antenna_longitude = vs1.antenna_longitude * w1 + vs2.antenna_longitude * w2
        if vs1.antenna_latitude_valid and vs2.antenna_latitude_valid:
            output.antenna_latitude_valid = True
            output.antenna_latitude = vs1.antenna_latitude * w1 + vs2.antenna_latitude * w2
        if vs1.antenna_altitude_valid and vs2.antenna_altitude_valid:
            output.antenna_altitude_valid = True
            output.antenna_altitude = vs1.antenna_altitude * w1 + vs2.antenna_altitude * w2
    else:
        output.longitude_valid = vs1.longitude_valid if w1 > w2 else vs2.longitude_valid
        output.longitude = vs1.longitude if w1 > w2 else vs2.longitude
        output.latitude_valid = vs1.latitude_valid if w1 > w2 else vs2.latitude_valid
        output.latitude = vs1.latitude if w1 > w2 else vs2.latitude
        output.altitude_valid = vs1.altitude_valid if w1 > w2 else vs2.altitude_valid
        output.altitude = vs1.altitude if w1 > w2 else vs2.altitude
        output.antenna_longitude_valid = vs1.antenna_longitude_valid if w1 > w2 else vs2.antenna_longitude_valid
        output.antenna_longitude = vs1.antenna_longitude if w1 > w2 else vs2.antenna_longitude
        output.antenna_latitude_valid = vs1.antenna_latitude_valid if w1 > w2 else vs2.antenna_latitude_valid
        output.antenna_latitude = vs1.antenna_latitude if w1 > w2 else vs2.antenna_latitude
        output.antenna_altitude_valid = vs1.antenna_altitude_valid if w1 > w2 else vs2.antenna_altitude_valid
        output.antenna_altitude = vs1.antenna_altitude if w1 > w2 else vs2.antenna_altitude
    output.satellite_count_valid = vs1.satellite_count_valid if w1 > w2 else vs2.satellite_count_valid
    output.satellite_count = vs1.satellite_count if w1 > w2 else vs2.satellite_count
    if vs1.speed_valid and vs2.speed_valid:
        output.speed_valid = True
        output.speed = vs1.speed * w1 + vs2.speed * w2
    if vs1.orientation_valid and vs2.orientation_valid:
        output.orientation_valid = True
        output.orientation = _interpolate_angle(vs1.orientation, w1, vs2.orientation, w2)
    if vs1.pitch_valid and vs2.pitch_valid:
        output.pitch_valid = True
        output.pitch = vs1.pitch * w1 + vs2.pitch * w2
    if vs1.roll_valid and vs2.roll_valid:
        output.roll_valid = True
        output.roll = vs1.roll * w1 + vs2.roll * w2
    if vs1.yaw_rate_valid and vs2.yaw_rate_valid:
        output.yaw_rate_valid = True
        output.yaw_rate = vs1.yaw_rate * w1 + vs2.yaw_rate * w2
    if vs1.ax_valid and vs2.ax_valid:
        output.ax_valid = True
        output.ax = vs1.ax * w1 + vs2.ax * w2
    if vs1.ay_valid and vs2.ay_valid:
        output.ay_valid = True
        output.ay = vs1.ay * w1 + vs2.ay * w2
    if vs1.az_valid and vs2.az_valid:
        output.az_valid = True
        output.az = vs1.az * w1 + vs2.az * w2
    if vs1.slip_angle_valid and vs2.slip_angle_valid:
        output.slip_angle_valid = True
        output.slip_angle = vs1.slip_angle * w1 + vs2.slip_angle * w2
    if vs1.jerk_x_valid and vs2.jerk_x_valid:
        output.jerk_x_valid = True
        output.jerk_x = vs1.jerk_x * w1 + vs2.jerk_x * w2
    if vs1.jerk_y_valid and vs2.jerk_y_valid:
        output.jerk_y_valid = True
        output.jerk_y = vs1.jerk_y * w1 + vs2.jerk_y * w2
    output.gnss_time_type = vs1.gnss_time_type
    if vs1.arrival_time_valid and vs2.arrival_time_valid:
        output.arrival_time_valid = True
        output.arrival_time = vs1.arrival_time * w1 + vs2.arrival_time * w2
    if vs1.gnss_time_valid and vs2.gnss_time_valid:
        diff_us = int((vs2.gnss_time - vs1.gnss_time).microseconds * w2 / (w1 + w2))
        output.gnss_time_valid = True
        output.gnss_time = vs1.gnss_time + timedelta(microseconds=diff_us)
    if vs1.pitch_rate_valid and vs2.pitch_rate_valid:
        output.pitch_rate_valid = True
        output.pitch_rate = vs1.pitch_rate * w1 + vs2.pitch_rate * w2
    if vs1.roll_rate_valid and vs2.roll_rate_valid:
        output.roll_rate_valid = True
        output.roll_rate = vs1.roll_rate * w1 + vs2.roll_rate * w2
    if vs1.horizontal_error_valid and vs2.horizontal_error_valid:
        output.horizontal_error_valid = True
        output.horizontal_error = vs1.horizontal_error * w1 + vs2.horizontal_error * w2
    if vs1.vertical_error_valid and vs2.vertical_error_valid:
        output.vertical_error_valid = True
        output.vertical_error = vs1.vertical_error * w1 + vs2.vertical_error * w2
    if vs1.curvature_valid and vs2.curvature_valid:
        output.curvature_valid = True
        output.curvature = vs1.curvature * w1 + vs2.curvature * w2
    return output


# GnssImuSample，用于样本输入
def get_gnssimu_sample(channel):
    vs1 = None
    vs2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v4 = 'gnssimu-sample-v4@' + str(channel)
    protocol_id_v5 = 'gnssimu-sample-v5@' + str(channel)
    protocol_id_v6 = 'gnssimu-sample-v6@' + str(channel)
    protocol_id_v7 = 'gnssimu-sample-v7@' + str(channel)
    if protocol_id_v7 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v7]
        vs1 = _conv_gnssimu_sample_v7(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v7(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v6 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v6]
        vs1 = _conv_gnssimu_sample_v6(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v6(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v5 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v5]
        vs1 = _conv_gnssimu_sample_v5(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v5(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        vs1 = _conv_gnssimu_sample_v4(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v4(pair.sample2)
        w2 = pair.weight2
    if vs1 is not None and vs2 is not None:
        return _interpolate_gnssimu_sample(vs1, w1, vs2, w2)
    return None
